﻿using UnityEngine;
using System.Collections;
using System.Collections.Generic;

public class Unit : MonoBehaviour {

	const float minPathUpdateTime = .2f;
	const float pathUpdateMoveThreshold = .5f;

	public Rigidbody2D rb;
	public Transform target;
	public float speed = 20;
	public float turnSpeed = 3;
	public float turnDst = 5;
	public float stoppingDst = 10;

	Path2D path;

	private void Awake() {
		if (target == null) {
			Debug.Log("not set target, try to find gameobject...");
			GameObject player = GameObject.FindGameObjectWithTag("Player");
			if (player != null)
				target = player.transform;
		}
		rb = gameObject.GetComponent<Rigidbody2D>();
	}

	void Start() {
		StartCoroutine(UpdatePath());
	}

	public void OnPathFound(Vector2[] waypoints, bool pathSuccessful) {
		if (pathSuccessful) {
			path = new Path2D(waypoints, transform.position, turnDst, stoppingDst);

			StopCoroutine("FollowPath");
			StartCoroutine("FollowPath");
		}
	}

	IEnumerator UpdatePath() {

		if (target == null)
			yield break;

		if (Time.timeSinceLevelLoad < .8f) {
			yield return new WaitForSeconds(.8f);
		}
		PathRequestManager.RequestPath(new PathRequest(transform.position, target.position, OnPathFound));

		float sqrMoveThreshold = pathUpdateMoveThreshold * pathUpdateMoveThreshold;
		Vector3 targetPosOld = target.position;

		while (true) {

			if (target == null)
				yield break;

			yield return new WaitForSeconds(minPathUpdateTime);
			//print(((target.position - targetPosOld).sqrMagnitude) + "    " + sqrMoveThreshold);
			if ((target.position - targetPosOld).sqrMagnitude > sqrMoveThreshold) {
				PathRequestManager.RequestPath(new PathRequest(transform.position, target.position, OnPathFound));
				targetPosOld = target.position;
			}
		}
	}

	IEnumerator FollowPath() {
		if (target == null)
			yield break;

		bool followingPath = true;
		int pathIndex = 0;

		float speedPercent = 1;

		while (followingPath) {

			if (target == null)
				yield break;

			Vector2 pos2D = new Vector2(transform.position.x, transform.position.y);
			while (Vector2.Distance(path.lookPoints[pathIndex], pos2D) < .1f) {
				if (pathIndex == path.finishLineIndex) {
					followingPath = false;
					rb.velocity = Vector2.zero;
					break;
				} else {
					pathIndex++;
				}
			}

			if (followingPath) {

				if (pathIndex >= path.slowDownIndex && stoppingDst > 0) {
					speedPercent = Mathf.Clamp01(path.turnBoundaries[path.finishLineIndex].DistanceFromPoint(pos2D) / stoppingDst);
					if (speedPercent < 0.01f) {
						followingPath = false;
						rb.velocity = Vector2.zero;
					}
				}
				rb.velocity = ((Vector3)path.lookPoints[pathIndex] - transform.position).normalized * Time.fixedDeltaTime * speed * speedPercent;
			}

			yield return null;

		}
	}

	float getTurnAngle(Vector2 targetPos) {
		Vector2 turnDir = targetPos - new Vector2(transform.position.x, transform.position.y);
		return -Mathf.Atan2(turnDir.x, turnDir.y) * Mathf.Rad2Deg;
	}

	private void OnDrawGizmos() {

		if (path != null) {
			path.DrawWithGizmos();
		}

	}
}
